/*************************************************************************************************************
 * Filename           : app_elevatorDispathch.c
 * Description        : Timer driver function file
*************************************************************************************************************/


/* Includes ------------------------------------------------------------------------------------------------*/
#include "bsp_led.h"
#include "bsp_timer.h"
#include "bsp_buzzer.h"
#include "bsp_stepMotor.h"
#include "app_elevatorDispatch.h"

/* Private typedef -----------------------------------------------------------------------------------------*/
/* Private define ------------------------------------------------------------------------------------------*/
/* Private macro -------------------------------------------------------------------------------------------*/
/* Private variables ---------------------------------------------------------------------------------------*/
FLOOR_DispatchTypeDef g_elevatorDispatch;

/* Private function prototypes -----------------------------------------------------------------------------*/
static void app_DelArrHead(void);
static void app_ElevatorInit(void);
static void app_DirLedControl(void);
static void app_ElevatorControl(void);
static void app_StepMotorControl(void);
static void app_RemoveFloor(uint8_t index);
static void app_AddFloor(uint8_t floorNum);
static void app_AddFloorNumUp(uint8_t floorNum);
static int app_FindElement(uint8_t elementToFind);
static void app_AddFloorNumDown(uint8_t floorNum);

/* Private functions ---------------------------------------------------------------------------------------*/


/************************************************************************************************************
 * Function Name   : app_BspInit
 * Description     : Configures system bsp
 * Input Variable  : void 
 * Return Variable : None
*************************************************************************************************************/
void app_BspInit(void)
{
  SystemInit();
  bsp_LedInit();
  bsp_TimerInit();
  bsp_ButtonInit();
  bsp_BuzzerInit();
  app_ElevatorInit();
  bsp_LixieTubeInit();
  bsp_StepMotorInit();
}


/************************************************************************************************************
 * Function Name   : app_ElevatorInit
 * Description     : Initialize Elevator relevent things
 * Input Variable  : void 
 * Return Variable : None
*************************************************************************************************************/
static void app_ElevatorInit(void)
{
  uint8_t floorNum = 0;
  
  g_elevatorDispatch.curDir          = ELEVATOR_DIR_NONE;
  g_elevatorDispatch.curFloor        = FLOOR1;
  g_elevatorDispatch.pressedNum      = 0u;
  g_elevatorDispatch.stopTimeCounter = 0u;
  for (floorNum = 0;floorNum < FLOOR_NUM;floorNum++)
  {
    g_elevatorDispatch.dispatchArr[floorNum] = 0u;
  }
}


/************************************************************************************************************
 * Function Name   : app_ElevatorDispatch
 * Description     : Execute elevator dispatch
 * Input Variable  : None
 * Return Variable : None
 * Note            : 
*************************************************************************************************************/
void app_ElevatorDispatch(void)
{
  bsp_ButtonScan();
  bsp_BuzzerControl();
  app_DirLedControl();
  app_ElevatorControl();
  app_StepMotorControl();
  
  bsp_NixieTubeDisplayFloor(g_elevatorDispatch.curFloor);
}


/************************************************************************************************************
 * Function Name   : app_ElevatorControl
 * Description     : Control the elevator to run or to stop
 * Input Variable  : None
 * Return Variable : None
 * Note            : 
*************************************************************************************************************/
static void app_ElevatorControl(void)
{
  if (g_elevatorDispatch.pressedNum != 0)
  {
    if (g_elevatorDispatch.stopTimeCounter == 0)
    {
      if (g_elevatorDispatch.curFloor < g_elevatorDispatch.dispatchArr[0])   
      {
        g_elevatorDispatch.curDir = ELEVATOR_DIR_UP;
        if (g_stepMotorControl.status == ELEVATOR_STOPPING)
        {
          bsp_stepMotorEnable();
          bsp_LedDispFloorNone();
        }
        else 
        {
          if (g_stepMotorControl.status  == ELEVATOR_ARRIVAL)
          {
            if (++g_elevatorDispatch.curFloor == g_elevatorDispatch.dispatchArr[0])
            {
              app_DelArrHead();
              bsp_BuzzerEnable();
              bsp_LedDispFloor(g_elevatorDispatch.curFloor);
              g_stepMotorControl.status = ELEVATOR_STOPPING;
              g_elevatorDispatch.stopTimeCounter = ELEVATOR_STOP_TIME;
            }
            else
            {
              bsp_stepMotorEnable();
              bsp_LedDispFloorNone();
            }
          }
        } /* End of else * if (g_stepMotorControl.status == ELEVATOR_STOPPING)    */
      } /* End of if (g_elevatorDispatch.curFloor < g_elevatorDispatch.dispatchArr[0])    */
      else if (g_elevatorDispatch.curFloor > g_elevatorDispatch.dispatchArr[0])  /* At the highest floor          */
      {
        g_elevatorDispatch.curDir = ELEVATOR_DIR_DOWN;
        if (g_stepMotorControl.status == ELEVATOR_STOPPING)
        {
          bsp_stepMotorEnable();
          bsp_LedDispFloorNone();
        }   
        else
        {
          if (g_stepMotorControl.status  == ELEVATOR_ARRIVAL)
          {
            if (--g_elevatorDispatch.curFloor == g_elevatorDispatch.dispatchArr[0])
            {
              app_DelArrHead();
              bsp_BuzzerEnable();
              bsp_LedDispFloor(g_elevatorDispatch.curFloor);
              g_stepMotorControl.status = ELEVATOR_STOPPING;
              g_elevatorDispatch.stopTimeCounter = ELEVATOR_STOP_TIME;
            }
            else
            {
              bsp_stepMotorEnable();
              bsp_LedDispFloorNone();
            }
          }
        }
      } /* End of else if (g_elevatorDispatch.curFloor > g_elevatorDispatch.dispatchArr[0]) */
    } /* End of if (g_elevatorDispatch.stopTimeCounter == 0)    */

    if (g_elevatorDispatch.curDir == ELEVATOR_DIR_DOWN)     /* When the elevator downward, it must stop in floor 1*/
    {
       if ((g_elevatorDispatch.pressedNum != 0) && (g_elevatorDispatch.curFloor != FLOOR1) && (app_FindElement(FLOOR1) == -1))
       {
         app_AddFloor(FLOOR1);
       }
    }
  } /* if (g_elevatorDispatch.pressedNum != 0)            */
  else
  {
    if (g_elevatorDispatch.stopTimeCounter == 0)
    {
      if (g_elevatorDispatch.curFloor != FLOOR1) 
      {
         g_stepMotorControl.status = ELEVATOR_STOPPING;
         g_elevatorDispatch.curDir = ELEVATOR_DIR_DOWN;
         app_AddFloor(FLOOR1);
      }
      else
      {
        g_elevatorDispatch.curDir = ELEVATOR_DIR_NONE;
        bsp_stepMotorDisable();
        bsp_LedDispFloorNone();
      }
    }
  }
}


/************************************************************************************************************
 * Function Name   : app_StepMotorControl
 * Description     : Control the step motor to forward or reverse
 * Input Variable  : None
 * Return Variable : None
 * Note            : 
*************************************************************************************************************/
static void app_StepMotorControl(void)
{
  if (g_elevatorDispatch.curDir == ELEVATOR_DIR_UP)
  {
    bsp_stepMotorUp();
  }
  else if (g_elevatorDispatch.curDir == ELEVATOR_DIR_DOWN)
  {
    bsp_stepMotorDown();
  }
  else 
  {
    bsp_stepMotorDisable();
  }
}


/************************************************************************************************************
 * Function Name   : app_DirLedControl
 * Description     : Control the direction LEDs
 * Input Variable  : None
 * Return Variable : None
*************************************************************************************************************/
static void app_DirLedControl(void)
{
  if (g_elevatorDispatch.curDir == ELEVATOR_DIR_UP)
  {
    bsp_LedDirUp();
  }
  else if (g_elevatorDispatch.curDir == ELEVATOR_DIR_DOWN)
  {
    bsp_LedDirDown();
  }
  else
  {
    bsp_LedDirNone();
  }
}


/************************************************************************************************************
 * Function Name   : app_AdjustElevatorDispatch
 * Description     : Basic on the user's requirement(press floor button) to change elevator dispatch
 * Input Variable  : btName  :  The ID of the button
 *                   btEvent :  The event of the button occur
 * Return Variable : None
 * Note            : 1 : If the floor button has been depressed, then BUTTON_DOUBLE_CLICK and BUTTON_LONG_CLICK
 *                       means to cancel the button, if has not been depressed, then all events view as select
 *                       the floor
 *                   2 : The floor number can not be remove when there is only 1 number in the array
*************************************************************************************************************/
void app_AdjustElevatorDispatch(BUTTON_IDTypeDef btName, BUTTON_EventTypeDef btEvent)
{
  int findIndicator = -1;
  if (btName == USER_BUTTON0)   /* Always ignore the 1 floor   */
  {
    return;
  }
  
  if (g_elevatorDispatch.pressedNum == 0)  
  {
    g_elevatorDispatch.dispatchArr[g_elevatorDispatch.pressedNum++] = btName + 1;
  }
  else
  {
    if (btEvent != BUTTON_SINGLE_CLICK)    /* See Note 1*/
    {
      findIndicator = app_FindElement(btName + 1);
      if ((findIndicator != -1) && (g_elevatorDispatch.pressedNum > 1))  /* See Note 2*/
      {
        app_RemoveFloor(findIndicator);
        return;
      }
    }
    
    app_AddFloor(btName + 1);
  }
}


/************************************************************************************************************
 * Function Name   : app_RemoveFloor
 * Description     : Remove a specific floor number from 'dispatchArr' array
 * Input Variable  : index : The index of 'dispatchArr' array
 * Return Variable : None
*************************************************************************************************************/
static void app_RemoveFloor(uint8_t index)
{
  uint8_t i = 0;
  
  if (index == (g_elevatorDispatch.pressedNum - 1))   /* At the tail  */
  {
    g_elevatorDispatch.dispatchArr[index] = 0;    
  }
  else 
  {
    for (i = index;i < g_elevatorDispatch.pressedNum;i++)
    {
      g_elevatorDispatch.dispatchArr[index] = g_elevatorDispatch.dispatchArr[index + 1];
    }
  }
  --g_elevatorDispatch.pressedNum;
}


/************************************************************************************************************
 * Function Name   : app_AddFloor
 * Description     : Add a specific floor number to 'dispatchArr' array
 * Input Variable  : floorNum : The number of the floor to add to 'dispatchArr' array
 * Return Variable : None
*************************************************************************************************************/
static void app_AddFloor(uint8_t floorNum)
{ 
  if (g_elevatorDispatch.curDir == ELEVATOR_DIR_UP)
  {
    app_AddFloorNumUp(floorNum);
  }
  else if (g_elevatorDispatch.curDir == ELEVATOR_DIR_DOWN)
  {
    app_AddFloorNumDown(floorNum);
  }
  else   /* Should not enter here*/
  {
    
  }
}


/************************************************************************************************************
 * Function Name   : app_AddFloorNumUp
 * Description     : Add a specific floor number to 'dispatchArr' array when the elevator is getting up
 * Input Variable  : floorNum : The number of the floor to add to 'dispatchArr' array
 * Return Variable : None
*************************************************************************************************************/
static void app_AddFloorNumUp(uint8_t floorNum)
{
  uint8_t i = 0;
  uint8_t j = 0;
  uint8_t k = 0;
  
  if (floorNum <= g_elevatorDispatch.curFloor)    /* Need to be handle when the elevator is down*/
  {
    for (i = 0;i < g_elevatorDispatch.pressedNum;i++)
    {
      if ((uint8_t)g_elevatorDispatch.dispatchArr[i] < (uint8_t)g_elevatorDispatch.curFloor)  /* Find another floor number smaller than current floor*/
      {
        for (j = i;j < g_elevatorDispatch.pressedNum;j++)
        {
          if (floorNum > g_elevatorDispatch.dispatchArr[j])  /* Get the floor num smaller than floor num, then put it here*/
          {
            for (k = g_elevatorDispatch.pressedNum; k > j; --k)
            {
              g_elevatorDispatch.dispatchArr[k] = g_elevatorDispatch.dispatchArr[k - 1];
            }
            g_elevatorDispatch.dispatchArr[k] = floorNum;
            ++g_elevatorDispatch.pressedNum;
            return;
          }
        }
      }
    } /* End of for (i = 0;i < g_elevatorDispatch.pressedNum;i++)*/

    if (i == g_elevatorDispatch.pressedNum)   /* Not found any floor number smaller than current floor      */
    {
      g_elevatorDispatch.dispatchArr[g_elevatorDispatch.pressedNum] = floorNum;
      ++g_elevatorDispatch.pressedNum;
    }
  } /* End of if (floorNum <= g_elevatorDispatch.curFloor)          */
  else  /* Can be handle when the elevator is up                    */
  {
    for (i = 0;i < g_elevatorDispatch.pressedNum;i++)
    {
      if (g_elevatorDispatch.dispatchArr[i] > floorNum)  /* Find another floor number bigger than floorNum  */
      {
        for (k = g_elevatorDispatch.pressedNum; k > i; --k)
        {
          g_elevatorDispatch.dispatchArr[k] = g_elevatorDispatch.dispatchArr[k - 1];
        }
        g_elevatorDispatch.dispatchArr[k] = floorNum;
        ++g_elevatorDispatch.pressedNum;
        return;
      }
    } /* End of for (i = 0;i < g_elevatorDispatch.pressedNum;i++)  */
    
    if (i == g_elevatorDispatch.pressedNum)   /* Not found any floor number bigger than floorNum */
    {
      for (j = 0;j < g_elevatorDispatch.pressedNum;j++)
      {
        if (g_elevatorDispatch.dispatchArr[j] < g_elevatorDispatch.curFloor)  /* Find another floor number smaller than current floor*/
        {
          for (k = g_elevatorDispatch.pressedNum; k > j; --k)
          {
            g_elevatorDispatch.dispatchArr[k] = g_elevatorDispatch.dispatchArr[k - 1];
          }
          g_elevatorDispatch.dispatchArr[k] = floorNum;
          ++g_elevatorDispatch.pressedNum;
          return;
        }
      } /* End of if (i == g_elevatorDispatch.pressedNum)   */

      if (j == g_elevatorDispatch.pressedNum)   /* Not found any floor number smaller than 'curFloor' */
      {
        g_elevatorDispatch.dispatchArr[g_elevatorDispatch.pressedNum] = floorNum;
        ++g_elevatorDispatch.pressedNum;
      }
    }
  } /* End of else of (if (floorNum <= g_elevatorDispatch.curFloor)) */
}


/************************************************************************************************************
 * Function Name   : app_AddFloorNumDown
 * Description     : Add a specific floor number to 'dispatchArr' array when the elevator is getting down
 * Input Variable  : floorNum : The number of the floor to add to 'dispatchArr' array
 * Return Variable : None
*************************************************************************************************************/
static void app_AddFloorNumDown(uint8_t floorNum)
{
  uint8_t i = 0;
  uint8_t j = 0;
  uint8_t k = 0;
  
  if (floorNum < g_elevatorDispatch.curFloor)    /* Can handle it in this round                             */
  {
    for (i = 0;i < g_elevatorDispatch.pressedNum;i++)
    {
      if (g_elevatorDispatch.dispatchArr[i] < floorNum)  /* Find another floor number smaller than floorNum */
      {
        for (k = g_elevatorDispatch.pressedNum; k > i; --k)
        {
          g_elevatorDispatch.dispatchArr[k] = g_elevatorDispatch.dispatchArr[k - 1];
        }
        g_elevatorDispatch.dispatchArr[k] = floorNum;
        ++g_elevatorDispatch.pressedNum;
        return;
      }
    }

    if (i == g_elevatorDispatch.pressedNum)   /* Not found any floor number smaller than floorNum           */
    {
      for (j = 0;j < g_elevatorDispatch.pressedNum;++j)  /* Find another floor number bigger than 'curFloor'*/
      {
        if ((uint8_t)g_elevatorDispatch.dispatchArr[j] > (uint8_t)g_elevatorDispatch.curFloor)
        {
          for (k = g_elevatorDispatch.pressedNum;k > j;--k)
          {
            g_elevatorDispatch.dispatchArr[k] = g_elevatorDispatch.dispatchArr[k - 1];
          }
          g_elevatorDispatch.dispatchArr[k] = floorNum;
          ++g_elevatorDispatch.pressedNum;
          return;
        }
      }
      if (j == g_elevatorDispatch.pressedNum)
      {
        g_elevatorDispatch.dispatchArr[g_elevatorDispatch.pressedNum] = floorNum;
        ++g_elevatorDispatch.pressedNum;
        return;
      }
    }
  } /* End of if (floorNum <= g_elevatorDispatch.curFloor)    */
  else  /* Need to be handle when the elevator is up      */
  {
    for (i = 0;i < g_elevatorDispatch.pressedNum;i++)
    {
      if ((uint8_t)g_elevatorDispatch.dispatchArr[i] >= (uint8_t)g_elevatorDispatch.curDir)  /* Find another floor number smaller than current floor*/
      {
        for (j = i;j < g_elevatorDispatch.pressedNum;j++)
        {
          if (floorNum < g_elevatorDispatch.dispatchArr[j])  /* Get the floor num smaller than floor num, then put it here*/
          {
            for (k = g_elevatorDispatch.pressedNum; k > j; --k)
            {
              g_elevatorDispatch.dispatchArr[k] = g_elevatorDispatch.dispatchArr[k - 1];
            }
            g_elevatorDispatch.dispatchArr[k] = floorNum;
            g_elevatorDispatch.pressedNum++;
            return;
          }
        }
      }
    } /* End of for (i = 0;i < g_elevatorDispatch.pressedNum;i++)         */
    
    if (i == g_elevatorDispatch.pressedNum)   /* Not found any floor number smaller than current floor      */
    {
      g_elevatorDispatch.dispatchArr[g_elevatorDispatch.pressedNum]  = floorNum;
      g_elevatorDispatch.pressedNum++;
      return;
    }
  }
}


/************************************************************************************************************
 * Function Name   : app_DelArrHead
 * Description     : Delete the head element of the 'dispatchArr' array
 * Input Variable  : None
 * Return Variable : None
*************************************************************************************************************/
static void app_DelArrHead(void)
{
  uint8_t i = 0;
  
  if (g_elevatorDispatch.pressedNum == 1)
  {
    g_elevatorDispatch.dispatchArr[0] = 0;
    g_elevatorDispatch.pressedNum = 0;
  }
  else 
  {
    for (i = 0;i < (g_elevatorDispatch.pressedNum - 1);++i)
    {
      g_elevatorDispatch.dispatchArr[i] = g_elevatorDispatch.dispatchArr[i + 1]; 
    }
    --g_elevatorDispatch.pressedNum;
  }
}


/************************************************************************************************************
 * Function Name   : app_FindElement
 * Description     : Check if there is the element i need in the 'dispatchArr' array
 * Input Variable  : elementToFind  :  The element 
 * Return Variable : placeIndex     : -1 if not exist
 *                                  : others if exist
*************************************************************************************************************/
static int app_FindElement(uint8_t elementToFind)
{
 uint8_t loopCount = 0;
 
 for (loopCount = 0;loopCount < g_elevatorDispatch.pressedNum;loopCount++)
 {
    if (g_elevatorDispatch.dispatchArr[loopCount] == elementToFind)
    {
      return loopCount;
    }
 }
 
 return -1;
}


/*************************************************END OF FILE************************************************/




